#include "PID.h"

void set_pid_max_duty(PIDStruct *pid, int16_t duty) {
    pid->max_duty = duty;
}

void set_pid_min_duty(PIDStruct *pid, int16_t duty) {
    pid->min_duty = duty;
}

void pid_update(PIDStruct *pid, float error) {
    pid->error1 = error;
    pid->d_duty = pid->i * pid->error1 + pid->p * (pid->error1 - pid->error2)
                  + pid->d * (pid->error1 - pid->error2 - pid->error2 + pid->error3);
    pid->duty += (int16_t)(pid->d_duty + 0.5f);
    if (pid->duty > pid->max_duty)
        pid->duty = pid->max_duty;
    if (pid->duty < pid->min_duty)
        pid->duty = pid->min_duty;
    pid->error3 = pid->error2;
    pid->error2 = pid->error1;
}
void pid_update_position(PIDStruct *pid, float error)
{
    pid->error1 += error;
    pid->error2 = error-pid->error3;
    pid->duty = (int16_t)(pid->p*error+pid->i*pid->error1+pid->d*pid->error2);
    pid->error3 = error;
}
void pid_clear(PIDStruct *pid) {
    pid->duty = 0;
    pid->d_duty = 0;
    pid->error1 = 0;
    pid->error2 = 0;
    pid->error3 = 0;
}

void set_p_i_d_value(PIDStruct *pid, float p, float i, float d) {
    pid->p = p;
    pid->i = i;
    pid->d = d;
}
